#line 1 "C:/Users/Carlo/Desktop/PROGETTI/tricopter/tric-trunk/tric.c"
#line 42 "C:/Users/Carlo/Desktop/PROGETTI/tricopter/tric-trunk/tric.c"
unsigned short THREAD_counts;
unsigned short THREAD_ready;


unsigned short MOTORS_speed[4];


int SENSORS_table_x[ 16 ];
int SENSORS_table_y[ 16 ];
int SENSORS_table_z[ 16 ];
int SENSORS_acceleration[3];


unsigned short RX_values[4];
unsigned short b_old;
unsigned short b_now;
unsigned short b_and;
#line 64 "C:/Users/Carlo/Desktop/PROGETTI/tricopter/tric-trunk/tric.c"
void Soft_UART_Write_Text(char* testo) {
 char cnt;
 for (cnt=0; cnt<strlen(testo); cnt++) {
 Soft_UART_Write(testo[cnt]);
 }
}
#line 73 "C:/Users/Carlo/Desktop/PROGETTI/tricopter/tric-trunk/tric.c"
void ADXL345_Wr(unsigned short address, unsigned short in) {
 I2C1_Start();
 I2C1_Wr(0x3A);
 I2C1_Wr(address);
 I2C1_Wr(in);
 I2C1_Stop();
}
#line 83 "C:/Users/Carlo/Desktop/PROGETTI/tricopter/tric-trunk/tric.c"
unsigned short ADXL345_Rd(unsigned short address) {
 unsigned short out;
 I2C1_Start();
 I2C1_Wr(0x3A);
 I2C1_Wr(address);
 I2C1_Stop();
 I2C1_Start();
 I2C1_Wr(0x3B);
 out = I2C1_Rd(0);
 I2C1_Stop();
 return out;
}
#line 100 "C:/Users/Carlo/Desktop/PROGETTI/tricopter/tric-trunk/tric.c"
void ADXL345_getAcc(int *acc) {
 unsigned short cnt;
 unsigned short acc_tmp[6];
 char z_string[8];

 I2C1_Start();
 I2C1_Wr(0x3A);
 I2C1_Wr(0x32);
 I2C1_Stop();
 I2C1_Start();
 I2C1_Wr(0x3B);
 for (cnt=0; cnt<6; cnt++) {
 char ack;
 ack = (cnt<5);
 acc_tmp[cnt] = I2C1_Rd(ack);
 }
 I2C1_Stop();

 acc[0] = acc_tmp[1];
 acc[0] = acc[0] << 8;
 acc[0] |= acc_tmp[0];
 acc[1] = acc_tmp[3];
 acc[1] = acc[1] << 8;
 acc[1] |= acc_tmp[2];
 acc[2] = acc_tmp[5];
 acc[2] = acc[2] << 8;
 acc[2] |= acc_tmp[4];
}
#line 131 "C:/Users/Carlo/Desktop/PROGETTI/tricopter/tric-trunk/tric.c"
void SENSORS_queue_init() {
 unsigned short cnt;
 int SENSORS_acceleration_now[3];
 for (cnt=0; cnt< 16 ; cnt++) {
 ADXL345_getAcc(&SENSORS_acceleration_now);
 SENSORS_table_x[cnt] = SENSORS_acceleration_now[0];
 SENSORS_table_y[cnt] = SENSORS_acceleration_now[1];
 SENSORS_table_z[cnt] = SENSORS_acceleration_now[2];
 Delay_ms(2);
 }
}
#line 145 "C:/Users/Carlo/Desktop/PROGETTI/tricopter/tric-trunk/tric.c"
void SENSORS_queue_update() {
 static unsigned short SENSORS_table_pointer;
 int acc_tmp[3];
 if (SENSORS_table_pointer < 0 || SENSORS_table_pointer >=  16 ) SENSORS_table_pointer = 0;
 ADXL345_getAcc(&acc_tmp);
 SENSORS_table_x[SENSORS_table_pointer] = acc_tmp[0];
 SENSORS_table_y[SENSORS_table_pointer] = acc_tmp[1];
 SENSORS_table_z[SENSORS_table_pointer] = acc_tmp[2];
 SENSORS_table_pointer++;
}
#line 158 "C:/Users/Carlo/Desktop/PROGETTI/tricopter/tric-trunk/tric.c"
int SENSORS_queue_filter(int *fifo) {
 unsigned short cnt;
 long value = 0;
 for (cnt=0; cnt< 16 ; cnt++) {
 value += fifo[cnt];
 }
 return (int)(value/ 16 );
}

void MOTORS_ctrl() {
 unsigned short cnt;
 unsigned short MOTORS_speed_max;
 MOTORS_speed_max = 0;
 THREAD_counts = 0;
 for(cnt=0; cnt<4; cnt++) {
 if(MOTORS_speed[cnt] >  95 ) MOTORS_speed[cnt] =  95 ;
 MOTORS_speed_max = (MOTORS_speed[cnt] >= MOTORS_speed_max) ? MOTORS_speed[cnt] : MOTORS_speed_max;
 }
 for(cnt=0; cnt<=MOTORS_speed_max; cnt++) {
 if(cnt == MOTORS_speed[0])  PORTC.B0  = 1;
 if(cnt == MOTORS_speed[1])  PORTC.B1  = 1;
 if(cnt == MOTORS_speed[2])  PORTC.B6  = 1;
 if(cnt == MOTORS_speed[3])  PORTC.B7  = 1;
 Delay_us(8);
 }
}
#line 188 "C:/Users/Carlo/Desktop/PROGETTI/tricopter/tric-trunk/tric.c"
void ESC_startup(){
 unsigned short cnt=0;
 while(cnt<200) {
  PORTC.B0  = 0;
  PORTC.B1  = 0;
  PORTC.B6  = 0;
 Delay_ms(1);
  PORTC.B0  = 1;
  PORTC.B1  = 1;
  PORTC.B6  = 1;
 Delay_ms(20);
 cnt++;
 }
 T0CON.TMR0ON=1;
}
#line 208 "C:/Users/Carlo/Desktop/PROGETTI/tricopter/tric-trunk/tric.c"
void ESC_config(){
 unsigned short cnt=0;
 while(cnt<150) {
  PORTC.B0  = 0;
  PORTC.B1  = 0;
  PORTC.B6  = 0;
 Delay_ms(2);
  PORTC.B0  = 1;
  PORTC.B1  = 1;
  PORTC.B6  = 1;
 Delay_ms(20);
 cnt++;
 }
 cnt = 0;
 while(cnt<150) {
  PORTC.B0  = 0;
  PORTC.B1  = 0;
  PORTC.B6  = 0;
 Delay_ms(1);
  PORTC.B0  = 1;
  PORTC.B1  = 1;
  PORTC.B6  = 1;
 Delay_ms(20);
 cnt++;
 }
 Delay_ms(2000);
}
#line 240 "C:/Users/Carlo/Desktop/PROGETTI/tricopter/tric-trunk/tric.c"
void SENSORS_update(){
 SENSORS_queue_update();
 SENSORS_acceleration[0] = SENSORS_queue_filter(&SENSORS_table_x);
 SENSORS_acceleration[1] = SENSORS_queue_filter(&SENSORS_table_y);
 SENSORS_acceleration[2] = SENSORS_queue_filter(&SENSORS_table_z);
}

void RX_speedPercent(){
 unsigned short cnt;
 float RX_temp;
 for(cnt=0; cnt<4; cnt++) {



 RX_temp = (float)(RX_values[cnt]) * 0.5;
 MOTORS_speed[cnt] = floor(RX_temp);



 }
}
#line 266 "C:/Users/Carlo/Desktop/PROGETTI/tricopter/tric-trunk/tric.c"
void PID_update() {
 static float PID_err_proportional_old[3];
 static float PID_err_integral[3];
 float PID_err_proportional[3];
 float PID_err_derivative[3];
 int PID_output[3];
 unsigned short cnt;

 for (cnt=0; cnt<3; cnt++) {

 PID_err_proportional[cnt] = RX_values[cnt] - SENSORS_acceleration[cnt];
 if(abs(PID_err_proportional[cnt]) >  0.01 ) PID_err_integral[cnt] += PID_err_proportional[cnt] *  0.002 ;
 PID_err_derivative[cnt] = (PID_err_proportional[cnt] - PID_err_proportional_old[cnt]) /  0.002 ;
 PID_output[cnt] = (int)( ( 0.1 *PID_err_proportional[cnt]) + ( 0.005 *PID_err_integral[cnt]) + ( 0.01 *PID_err_derivative[cnt]) );

 if(PID_output[cnt] >  100 ) PID_output[cnt] =  100 ;
 else if(PID_output[cnt] <  0 ) PID_output[cnt] =  0 ;

 PID_err_proportional_old[cnt] = PID_err_proportional[cnt];
 }
 MOTORS_speed[0] = PID_output[1] + PID_output[2] - PID_output[0];
 MOTORS_speed[1] = PID_output[2] - PID_output[1] - PID_output[1];
 MOTORS_speed[2] = PID_output[0] + PID_output[1] + PID_output[2];
}

unsigned short oldLineState[4];
#line 296 "C:/Users/Carlo/Desktop/PROGETTI/tricopter/tric-trunk/tric.c"
void main() {


  TRISC.B2  = 0;
  TRISC.B0  = 0;
  TRISC.B1  = 0;
  TRISC.B6  = 0;
  TRISC.B7  = 0;
  TRISB.B4  = 1;
  TRISB.B5  = 1;
  TRISB.B6  = 1;
  TRISB.B7  = 1;
  PORTC.B2  = 0;


 ADCON1 = 0x0F;
 CMCON = 0xCF;




  TRISA.B0  = 0;
 Soft_UART_Init(&PORTA, 1, 0, 57600, 0);
 I2C1_Init(400000);
  PORTC.B0  = 1;
  PORTC.B1  = 1;
  PORTC.B6  = 1;
  PORTC.B7  = 1;
 Delay_ms(500);



 ADXL345_Wr(0x2C, 0x0C);
 ADXL345_Wr(0x2D, 0x08);
 ADXL345_Wr(0x31, 0x0B);
 Delay_ms(500);


 T0CON.T0PS0 = 0;
 T0CON.T0PS1 = 1;
 T0CON.T0PS2 = 1;
 T0CON.PSA = 1;
 T0CON.T0CS = 0;
 T0CON.T08BIT = 1;
 INTCON.TMR0IE = 0;
 TMR0L = 0;
 TMR0H = 0;
 INTCON.TMR0IF = 0;
 T0CON.TMR0ON = 0;


 T1CON.T1CKPS1 = 0;
 T1CON.T1CKPS0 = 0;
 INTCON.GIE = 1 ;
 INTCON.PEIE = 1 ;
 PIE1.TMR1IE = 1 ;
 TMR1H = 0xD1;
 TMR1L = 0x80;
 PIR1.TMR1IF = 0 ;
 T1CON.TMR1ON = 1;


 INTCON.RBIE = 1;
 INTCON.RBIF = 0;
 INTCON.INT0IE = 0;
 INTCON.INT0IF = 0;



 THREAD_counts = 0;
 THREAD_ready = 0;
 MOTORS_speed[0] = 0;
 MOTORS_speed[1] = 0x20;
 MOTORS_speed[2] = 0x30;
 MOTORS_speed[3] = 0x60;
 b_old = PORTB;
 b_now = PORTB;



 SENSORS_queue_init();
 ESC_config();
 ESC_startup();

 T0CON.TMR0ON = 1;

 while (1) {
 if(THREAD_ready){
 switch(THREAD_counts){
 case 1:
  PORTC.B2 =! PORTC.B2 ;
 THREAD_ready = 0;
 break;

 case 20:
  PORTC.B0  = 0;
  PORTC.B1  = 0;
  PORTC.B6  = 0;
  PORTC.B7  = 0;
 THREAD_ready = 0;
 break;
 case 0:
 case 2:
 case 4:
 case 6:
 case 8:
 case 10:
 case 12:
 case 14:
 case 16:
 case 18:
 RX_speedPercent();
 SENSORS_update();

 THREAD_ready = 0;
 break;

 case 21:
 MOTORS_ctrl();
 THREAD_ready = 0;
 break;
 case 3:
 case 5:
 case 7:
 case 9:
 case 11:
 case 13:
 case 15:
 case 17:
 case 19:
 THREAD_ready = 0;
 break;

 }
 }
 }
}


void interrupt() {

 if(INTCON.RBIF){
 INTCON.RBIE = 0;
 b_now = PORTB;
 b_and = b_now ^ b_old;


 if(b_and & 0x010) {
 if(b_now & 0x010) {
 RX_values[0] = (TMR0L - RX_values[0]);


 } else {

 RX_values[0] = TMR0L;
 }
 }
 if(b_and & 0x020) {
 if(b_now & 0x020) {
 RX_values[1] = TMR0L - RX_values[1];
 } else {
 RX_values[1] = TMR0L;
 }
 }
 if(b_and & 0x040) {
 if(b_now & 0x040) {
 RX_values[2] = TMR0L - RX_values[2];
 } else {
 RX_values[2] = TMR0L;
 }
 }
 if(b_and & 0x080) {
 if(b_now & 0x080) {
 RX_values[3] = TMR0L - RX_values[3];
 } else {
 RX_values[3] = TMR0L;
 }
 }

 b_old = b_now;
 INTCON.RBIF = 0;
 INTCON.RBIE = 1;
 }

 if(PIR1.TMR1IF) {
 TMR1H = 0xD1;
 TMR1L = 0x80;
 PIR1.TMR1IF=0;
 THREAD_counts++;
 THREAD_ready = 1;
 }
#line 494 "C:/Users/Carlo/Desktop/PROGETTI/tricopter/tric-trunk/tric.c"
 }
